

import sys
import time

import rclpy
from rclpy.node import Node

from tornado.httpserver import HTTPServer
from tornado.ioloop import IOLoop, PeriodicCallback
from tornado.netutil import bind_sockets
from tornado.web import Application

from websocket_handler import  Ros2WebSocket


def start_hook():
    IOLoop.instance().start()


def shutdown_hook():
    IOLoop.instance().stop()


class Ros2WebsocketNode(Node):
    def __init__(self):
        super().__init__("ros2webserver")

        Ros2WebSocket.node_handle = self

        # get tornado application parameters
        tornado_settings = {}
        tornado_settings["websocket_ping_interval"] = float(
            self.declare_parameter("websocket_ping_interval", 0).value
        )
        tornado_settings["websocket_ping_timeout"] = float(
            self.declare_parameter("websocket_ping_timeout", 30).value
        )
        if "--websocket_ping_interval" in sys.argv:
            idx = sys.argv.index("--websocket_ping_interval") + 1
            if idx < len(sys.argv):
                tornado_settings["websocket_ping_interval"] = float(sys.argv[idx])
            else:
                print("--websocket_ping_interval argument provided without a value.")
                sys.exit(-1)

        if "--websocket_ping_timeout" in sys.argv:
            idx = sys.argv.index("--websocket_ping_timeout") + 1
            if idx < len(sys.argv):
                tornado_settings["websocket_ping_timeout"] = float(sys.argv[idx])
            else:
                print("--websocket_ping_timeout argument provided without a value.")
                sys.exit(-1)

        # Server and SSL options
        # SSL options, cannot set default to None - rclpy throws warning
        certfile = self.declare_parameter("certfile", "").value
        keyfile = self.declare_parameter("keyfile", "").value
        # if not set, set to None
        if certfile == "":
            certfile = None
        if keyfile == "":
            keyfile = None

        port = self.declare_parameter("port", 9091).value
        if "--port" in sys.argv:
            idx = sys.argv.index("--port") + 1
            if idx < len(sys.argv):
                port = int(sys.argv[idx])
            else:
                print("--port argument provided without a value.")
                sys.exit(-1)
        address = self.declare_parameter("address", "").value
        if "--address" in sys.argv:
            idx = sys.argv.index("--address") + 1
            if idx < len(sys.argv):
                address = sys.argv[idx]
            else:
                print("--address argument provided without a value.")
                sys.exit(-1)

        url_path = self.declare_parameter("url_path", "/").value
        if "--url_path" in sys.argv:
            idx = sys.argv.index("--url_path") + 1
            if idx < len(sys.argv):
                url_path = str(sys.argv[idx])
            else:
                print("--url_path argument provided without a value.")
                sys.exit(-1)

        retry_startup_delay = self.declare_parameter("retry_startup_delay", 2.0).value  # seconds.
        if "--retry_startup_delay" in sys.argv:
            idx = sys.argv.index("--retry_startup_delay") + 1
            if idx < len(sys.argv):
                retry_startup_delay = int(sys.argv[idx])
            else:
                print("--retry_startup_delay argument provided without a value.")
                sys.exit(-1)

        ##################################################
        # Done with parameter handling                   #
        ##################################################

        handlers = [(r"/", Ros2WebSocket), (r"", Ros2WebSocket)]
        if url_path != "/":
            handlers = [(rf"{url_path}", Ros2WebSocket)]

        application = Application(handlers, **tornado_settings)

        connected = False
        while not connected and self.context.ok():
            try:
                ssl_options = None
                if certfile is not None and keyfile is not None:
                    ssl_options = {"certfile": certfile, "keyfile": keyfile}
                sockets = bind_sockets(port, address)
                actual_port = sockets[0].getsockname()[1]
                server = HTTPServer(application, ssl_options=ssl_options)
                server.add_sockets(sockets)
                self.declare_parameter("actual_port", actual_port)
                self.get_logger().info(f"Ros2 WebSocket server started on port {actual_port}")
                connected = True
            except OSError as e:
                self.get_logger().warn(
                    "Unable to start server: {} " "Retrying in {}s.".format(e, retry_startup_delay)
                )
                time.sleep(retry_startup_delay)

def main(args=None):
    if args is None:
        args = sys.argv

    rclpy.init(args=args)
    node = Ros2WebsocketNode()

    executor = rclpy.executors.SingleThreadedExecutor()
    executor.add_node(node)
    spin_callback = PeriodicCallback(lambda: executor.spin_once(timeout_sec=0.01), 1)
    spin_callback.start()
    try:
        start_hook()
        node.destroy_node()
        rclpy.shutdown()
    except KeyboardInterrupt:
        print("Exiting due to SIGINT")
    finally:
        shutdown_hook()  # shutdown hook to stop the server


if __name__ == "__main__":
    main()
